/**
  ******************************************************************************
  * @file    main.c
  * @author  luoxiao
  * @version V0.0
  * @date    2019/07/22
  * @brief   Program for INS
  ******************************************************************************
  * @attention
  *
  *
  ******************************************************************************
  */
  
/* Kernel includes.*/
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

/* Hardware and debug kit includes */
#include "bsp_debug_uart.h"
#include "bsp_debug_led.h"
#include "mpu_iic.h"
#include "imu.h"


/* 
* Create Task Handler 
*/
TaskHandle_t AppTaskCreate_Handle = NULL;
TaskHandle_t Task_ReadData_Handle = NULL;
TaskHandle_t Task_AHRS_Handle = NULL;

/* 
 * Create Kernel object handler 
 */
SemaphoreHandle_t SemReadData_Handle;
QueueHandle_t QueSensorData_Handle;

/* 
 * function declaration 
 */
static void prvSetupHardware(void);
static void AppTaskCreate(void);
/*----------------------------------------------------------------*/

/*
  * @brief  main function
  * @param  non
  * @retval non
  */
int main(void)
{	
	BaseType_t xReturn = pdPASS; //define a return value, pdPASS by default
	
	/*Board start process*/
	prvSetupHardware();
	
	/*create AppTaskCreate task*/
	xReturn = xTaskCreate((TaskFunction_t)AppTaskCreate, 
								(const char*	)"AppTaskCreate",			
								(uint16_t		)512,						
								(void*			)NULL,						
								(UBaseType_t	)1,							
								(TaskHandle_t*	)&AppTaskCreate_Handle);	
	if(pdPASS == xReturn)
	{
		printf("AppTaskCreate create suceed.\r\n");
		vTaskStartScheduler();
	}
	else
		return -1;
	
	while(1);
}
/*----------------------------------------------------------------*/

/**
  * @brief  board peripherals init
  * @param  non
  * @retval non
  */
static void prvSetupHardware(void)
{
	/*4 bits for pre-emption priority, 0 bits for subpriority*/
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
	
	/*LED init*/
	LED_Init();

	/*USART1 init as 115200 8-N-1*/
	UART1_Init();
	
}
/*----------------------------------------------------------------*/

/**
  * @brief  AppTaskCreate
  * @param  non
  * @retval non
  */
static void AppTaskCreate(void)
{
	BaseType_t xReturn = pdPASS; 
	
	taskENTER_CRITICAL(); //enter critical zone
	
	/* create Semphera*/
	SemReadData_Handle = xSemaphoreCreateBinary();
	if(NULL != SemReadData_Handle)
		printf("SemReadData_Handle create succeed.\r\n");
	
	/* create Queue*/
	QueSensorData_Handle = xQueueCreate(8, sizeof(SensorData));
	
	/*create read imu task*/
	xReturn= xTaskCreate((TaskFunction_t )Task_ReadData,
						(const char*	)"Task_ReadData",
						(uint16_t		)512, 
						(void* 			)NULL, 
						(UBaseType_t	)2, 
						(TaskHandle_t*	)&Task_ReadData_Handle);
	if(pdPASS ==  xReturn)	//create succeed
		printf("Task_ReadData create suceed.\r\n");
	else
		printf("Task_ReadData create failed.\r\n");
	
	/*create ahrs task*/
	xReturn= xTaskCreate((TaskFunction_t )Task_AHRS,
						(const char*	)"Task_AHRS",
						(uint16_t		)512, 
						(void* 			)NULL, 
						(UBaseType_t	)3, 
						(TaskHandle_t*	)&Task_AHRS_Handle);
	if(pdPASS ==  xReturn)	//create succeed
		printf("Task_AHRS create suceed.\r\n");
	else
		printf("Task_AHRS create failed.\r\n");
	
	vTaskDelete(AppTaskCreate_Handle); //delete AppTaskCreate task
	printf("AppTaskCreate deleted.\r\n");
	
	taskEXIT_CRITICAL();//exit critical zone
}
